#ifndef SAMPLEACTION_H
#define SAMPLEACTION_H

#include <tsubamegaeshi-base/action.h>

class Sampleaction :public Action
{
  enum BehaviorState
  {
    ARM_UP = 0,
    ARM_ROTATE = 1,
    ARM_WAVE_1 = 2,
    ARM_WAVE_2 = 3
  };

public:
  Sampleaction(Worldmodel::shared_ptr world) : Action(world) {}
  ~Sampleaction(){};

  bool action() {
    Worldmodel::TStatus& wst = world_->current_status();

    //wst.dump();

    if (wst.gs_pm_ != "BeforeKickOff") {
	//    if (false) {
        static const float gain = 0.1;
        static BehaviorState leftstate = ARM_UP;
        static BehaviorState rightstate = ARM_UP;

        float curAngle = 0;
        float newAngle = 0;
 
        switch(rightstate) {
        case ARM_UP:
            curAngle = wst.uj_["raj1_2"].second;
            if (curAngle > - 90.0) {
                newAngle = gain * (-90.0 - curAngle);
                rae1_2(0, newAngle);
            } else {
                rightstate = ARM_ROTATE;
            }
            break;
        case ARM_ROTATE:
            curAngle = wst.hj_["raj3"];
            if (curAngle > -90.0) {
                newAngle = gain * (-90.0 - curAngle);
                rae3(newAngle);
            } else {
                rightstate = ARM_WAVE_1;
            }
            break;
        case ARM_WAVE_1:
            curAngle = wst.hj_["raj4"];
            if (curAngle < 90.0) {
                newAngle = gain * (90.0 - curAngle);
                rae4(newAngle);
            } else {
                rightstate = ARM_WAVE_2;
            }
            break;
        case ARM_WAVE_2:
            curAngle = wst.hj_["raj4"];
            if (curAngle > 45.0 || curAngle <= 44.5)
            {
                newAngle = gain * (45.0 - curAngle);
                rae4(newAngle);
            } else {
                rightstate = ARM_WAVE_1;
            }
            break;
        default:   
            break;
        }
        
        switch(leftstate) {
        case ARM_UP:
            curAngle = wst.uj_["laj1_2"].second;
            if (curAngle < 90.0) {
                newAngle = gain * (90.0 - curAngle);
                lae1_2(0, newAngle);
            } else {
                leftstate = ARM_ROTATE;
            }
            break;
        case ARM_ROTATE:
            curAngle = wst.hj_["laj3"];
            if (curAngle < 90.0) {
                newAngle = gain * (90.0 - curAngle);
                lae3(newAngle);
            } else {
                leftstate = ARM_WAVE_1;
            }
            break;
        case ARM_WAVE_1:
            curAngle = wst.hj_["laj4"];
            if (curAngle < 90.0) {
                newAngle = gain * (90.0 - curAngle);
                lae4(newAngle);
            } else {
                leftstate = ARM_WAVE_2;
            }
            break;
        case ARM_WAVE_2:
            curAngle = wst.hj_["laj4"];
            if (curAngle > 45.0 || curAngle <= 44.5) {
                newAngle = gain * (45.0 - curAngle);
                lae4(newAngle);
            } else {
                leftstate = ARM_WAVE_1;
            }
            break;
        default:   
            break;
        }
    }
    
    return true;
  }
};

#endif//SAMPLEACTION_H
